#include "maquina_tx.h"
#include <time.h>
#include <QDebug>

//*************** CONFIGURACIONES PUERTO ***********************
int USB_port;

//*************** VARIABLES RECEPCION **************************
int32_t USB_RX_bytes_disponibles;  //Bytes disponibles para recibir de USB
uint8_t USB_RX_buffer[255];				//Buffer de almacenaje temporal
uint32_t USB_RX_pila[260];
uint32_t USB_RX_bytes_restantes;
boolean USB_RX_stuffing = false;
uint32_t USB_RX_indice = 0;
uint32_t USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
boolean USB_RX_recibiendo = false;
boolean USB_RX_crc_OK = false;
time_t USB_RX_byte_timer;
time_t USB_RX_response_timer;

//******************* VARIABLES EMISION ****************************
boolean USB_TX_enviando = false;
uint32_t USB_TX_pila[260];
uint32_t USB_TX_estado = HSCAR_COMM_ESTADO_CABECERA;
uint32_t USB_TX_indice = 0;
boolean USB_TX_stuffing = false;
uint32_t USB_TX_bytes_restantes = 0;
boolean USB_TX_algo_por_enviar = true;

char CRC_Local;
char CRC_Recibido;

char mensaje_loopback[] = "Pepito conejo al monte escapo";

int inicializarPuerto(char *port, uint32_t baudrate)
{
    struct termios options;
    USB_port = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
    //~ close(USB_port);

    if (USB_port != -1)
    {
        //~ printf("Port succesfully added /dev/ttyUSB on %d", USB_port);
        //~ fflush(stdout);
        fcntl(USB_port, F_SETFL, O_NONBLOCK);               //Configure to be blocking
        tcgetattr(USB_port, &options);                      //Get the current options for the port
        cfsetispeed(&options, baudrate);                    //Set input speed to baudrate
        cfsetospeed(&options, baudrate);                    //Set output speed to baudrate
        options.c_cflag |= (CLOCAL | CREAD);                //Enable receiver and only local line
        options.c_cflag &= ~PARENB;                         //No parity checking
        options.c_cflag &= ~CSTOPB;                         //Only one stop bit
        options.c_cflag &= ~CSIZE;                          //Reset the character size
        options.c_cflag |= CS8;                             //Set the character size to 8
        options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); //Setting raw imput
        options.c_iflag &= ~(IXON | IXOFF | IXANY);         //Unable flow control
        tcsetattr(USB_port, TCSAFLUSH, &options);           //Writing the configurations and flushing the buffers
        sleep(1);
    }
    return USB_port;
}

void enviarUSB() {
    uint8_t tx_byte;

    USB_TX_estado = HSCAR_COMM_ESTADO_CABECERA;
    USB_TX_indice = 0;

    do {
        if (HSCAR_COMM_ES_COMANDO_LARGO(USB_TX_pila[0]) || HSCAR_COMM_ES_COMANDO_CORTO(USB_TX_pila[0])) {

            switch (USB_TX_estado) {
            //Enviando cabecera
            case HSCAR_COMM_ESTADO_CABECERA:
                tx_byte = (0xFF & HSCAR_COMM_CABECERA);
                write(USB_port, &tx_byte, 1);
                USB_TX_estado = HSCAR_COMM_ESTADO_COMANDO;
                break;

                //Enviando comando
            case HSCAR_COMM_ESTADO_COMANDO:
                tx_byte = USB_TX_pila[USB_TX_indice++];
                write(USB_port, &tx_byte, 1);

                if (HSCAR_COMM_ES_COMANDO_LARGO(tx_byte))
                    USB_TX_estado = HSCAR_COMM_ESTADO_NBYTES;
                else if (HSCAR_COMM_ES_COMANDO_CORTO(tx_byte))
                    USB_TX_estado = HSCAR_COMM_ESTADO_CRC;
                break;

                //Enviamos el numero de bytes
            case HSCAR_COMM_ESTADO_NBYTES:
                tx_byte = USB_TX_pila[USB_TX_indice++];
                write(USB_port, &tx_byte, 1);

                if ((!USB_TX_stuffing) && (((0xFF) & tx_byte) == HSCAR_COMM_CABECERA)) {
                    USB_TX_stuffing = true;
                    USB_TX_indice--;

                }else if (!USB_TX_stuffing) {
                    USB_TX_estado = HSCAR_COMM_ESTADO_NO_NBYTES;
                    USB_TX_bytes_restantes = tx_byte;

                }else {
                    USB_TX_stuffing = 0;
                    USB_TX_estado = HSCAR_COMM_ESTADO_NO_NBYTES;
                    USB_TX_bytes_restantes = tx_byte;
                }

                break;

                //Enviamos el numbero de bites negado
            case HSCAR_COMM_ESTADO_NO_NBYTES:
                tx_byte = USB_TX_pila[USB_TX_indice++];
                write(USB_port, &tx_byte, 1);

                if ((USB_TX_stuffing == 0) && (((0xFF) & tx_byte) == ((0xFF) & HSCAR_COMM_CABECERA))) {
                    USB_TX_stuffing = 1;
                    USB_TX_indice--;

                }else if (USB_TX_stuffing == 1){
                    USB_TX_stuffing = 0;
                }

                //Si no estamos rellenando o el relleno ha terminado
                if (USB_TX_stuffing == 0) {
                    USB_TX_estado = HSCAR_COMM_ESTADO_BUCLE;
                }

                break;

                //Enviamos los datos extra que incluye la trama
            case HSCAR_COMM_ESTADO_BUCLE:
                tx_byte = USB_TX_pila[USB_TX_indice++];
                write(USB_port, &tx_byte, 1);

                if ((!USB_TX_stuffing) && (((0xFF) & tx_byte) == HSCAR_COMM_CABECERA)) {
                    USB_TX_stuffing = true;
                    USB_TX_indice--;
                }else if (USB_TX_stuffing){
                    USB_TX_stuffing = false;
                }

                //Si no estamos rellenando o el rellenado ha terminado y no quedan mas bytes
                if ((!USB_TX_stuffing) && ((--USB_TX_bytes_restantes) == 0)) {
                    USB_TX_estado = HSCAR_COMM_ESTADO_CRC;
                }

                break;


                //Calculamos el CRC (habria que tener en cuenta el rellenado tambien)
            case HSCAR_COMM_ESTADO_CRC:

                if (HSCAR_COMM_ES_COMANDO_CORTO(USB_TX_pila[0])) //Si es comando corto, solo hay un byte al que hacer CRC
                {
                    USB_TX_pila[USB_TX_indice++] = CRC_CalcCRC(USB_TX_pila[0]);
                }
                else if (HSCAR_COMM_ES_COMANDO_LARGO(USB_TX_pila[0])) //Si es comando largo, hay que hacer CRC a todo el array (A partir del comando y hasta el ultimo
                {
                    USB_TX_pila[USB_TX_indice] = CRC_CalcBlockCRC(USB_TX_pila, USB_TX_indice);
                    USB_TX_indice++;
                }

                write(USB_port, &USB_TX_pila[USB_TX_indice - 1], 1);

                if ((!USB_TX_stuffing) && (USB_TX_pila[USB_TX_indice - 1] == HSCAR_COMM_CABECERA)) {
                    USB_TX_stuffing = true;
                    USB_TX_indice--;

                }else {
                    //printf("\n%02X", HSCAR_COMM_CABECERA);
                    for(uint32_t i = 0; i < USB_TX_indice; i++) {
                        //printf(":%02X", USB_TX_pila[i]);
                    }
                    //printf("\n");
                    fflush(stdout);

                    USB_TX_stuffing = false;
                    USB_TX_estado = HSCAR_COMM_ESTADO_CABECERA;
                    USB_TX_indice = 0;
                }
                break;

            default:
                USB_TX_estado = HSCAR_COMM_ESTADO_CABECERA;
                USB_TX_indice = 0;
                break;
            }
        }
    } while (USB_TX_estado != HSCAR_COMM_ESTADO_CABECERA);
}

void leerUSB(int32_t nbytes) {

    for (int32_t i = 0 ; (i < nbytes) && USB_RX_recibiendo; i++) {
        uint8_t rx_byte = USB_RX_buffer[i];

        USB_RX_pila[USB_RX_indice++] = (0xFF & rx_byte);

        //        printf("%02d:", (0xFF & rx_byte));
        //        fflush(stdout);

        switch(USB_RX_estado) {
        case HSCAR_COMM_ESTADO_CABECERA:

            if ((0xFF & rx_byte) == HSCAR_COMM_CABECERA) {
                USB_RX_estado = HSCAR_COMM_ESTADO_COMANDO;
                USB_RX_stuffing = false;
                USB_RX_crc_OK = false;

            }else {
                USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
            }

            USB_RX_indice = 0;
            break;

        case HSCAR_COMM_ESTADO_COMANDO:

            if (HSCAR_COMM_ES_COMANDO_LARGO(0xFF & rx_byte)) {
                USB_RX_estado = HSCAR_COMM_ESTADO_NBYTES;

            }else if (HSCAR_COMM_ES_COMANDO_CORTO(0xFF & rx_byte)) {
                USB_RX_estado = HSCAR_COMM_ESTADO_CRC;

            }else {
                USB_RX_recibiendo = false;
                USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
                USB_RX_indice = 0;
            }
            break;

        case HSCAR_COMM_ESTADO_NBYTES:

            if ((!USB_RX_stuffing) && ((0xFF & rx_byte) == HSCAR_COMM_CABECERA)) {
                USB_RX_stuffing = true;
                USB_RX_indice--;

            }else if ((USB_RX_stuffing) && ((0xFF & rx_byte) != HSCAR_COMM_CABECERA)) {
                USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
                USB_RX_indice = 0;
                USB_RX_recibiendo = false;

            }else {
                USB_RX_stuffing = false;
                USB_RX_estado = HSCAR_COMM_ESTADO_NO_NBYTES;

            }
            break;

        case HSCAR_COMM_ESTADO_NO_NBYTES:

            if ((!USB_RX_stuffing) && ((0xFF & rx_byte) == HSCAR_COMM_CABECERA)) {
                USB_RX_stuffing = true;
                USB_RX_indice--;

            }else if ((USB_RX_stuffing) && ((0xFF & rx_byte) != HSCAR_COMM_CABECERA)) {
                USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
                USB_RX_indice = 0;
                USB_RX_stuffing = false;
                USB_RX_recibiendo = false;

            }else if ((0xFF & rx_byte) == (0xFF & (~USB_RX_pila[USB_RX_indice - 2]))){
                USB_RX_stuffing = false;
                USB_RX_bytes_restantes = (0xFF & USB_RX_pila[USB_RX_indice - 2]);
                USB_RX_estado = HSCAR_COMM_ESTADO_BUCLE;

            }else {
                USB_RX_stuffing = false;
                USB_RX_indice = 0;
                USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
                USB_RX_recibiendo = false;
            }

            break;

        case HSCAR_COMM_ESTADO_BUCLE:

            if ((!USB_RX_stuffing) && ((0xFF & rx_byte) == HSCAR_COMM_CABECERA)) {
                USB_RX_stuffing = true;
                USB_RX_indice--;

            }else if ((USB_RX_stuffing) && ((0xFF & rx_byte) != HSCAR_COMM_CABECERA)) {
                USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
                USB_RX_indice = 0;
                USB_RX_recibiendo = false;

            }else {
                USB_RX_stuffing = false;

                if ((--USB_RX_bytes_restantes) == 0) {
                    USB_RX_estado = HSCAR_COMM_ESTADO_CRC;
                }

            }
            break;

        case HSCAR_COMM_ESTADO_CRC:

            if ((!USB_RX_stuffing) && ((0xFF & rx_byte) == HSCAR_COMM_CABECERA)) {
                USB_RX_stuffing = true;
                USB_RX_indice--;

            }else if ((USB_RX_stuffing) && ((0xFF & rx_byte) != HSCAR_COMM_CABECERA)) {
                USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
                USB_RX_indice = 0;
                USB_RX_recibiendo = false;

            }else {
                USB_RX_stuffing = false;

                if (HSCAR_COMM_ES_COMANDO_CORTO(USB_RX_pila[0]))	//Si es comando corto, solo hay un byte al que hacer CRC
                {
                    CRC_Local 	 = CRC_CalcCRC(0xFF & USB_RX_pila[0]) & 0xFF;
                    CRC_Recibido = USB_RX_pila[1];
                    USB_RX_crc_OK = (CRC_Local == CRC_Recibido);
                }
                else if (HSCAR_COMM_ES_COMANDO_LARGO(USB_RX_pila[0])) //Si es comando largo, hay que hacer CRC a todo el array (A partir del comando y hasta el ultimo byte)
                {
                    CRC_Local = CRC_CalcBlockCRC(USB_RX_pila, USB_RX_indice - 1) & 0xFF;
                    CRC_Recibido = USB_RX_pila[USB_RX_indice - 1];
                    USB_RX_crc_OK = (CRC_Local == CRC_Recibido);
                }
                else
                    USB_RX_crc_OK = false;

//                printf("\nCRC Recibido:%02X\nCRC Calculado:%02X", CRC_Recibido, CRC_Local);
//                fflush(stdout);
                USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
                USB_RX_indice = 0;
                USB_RX_recibiendo = false;
            }
            break;

        default:
            USB_RX_indice = 0;
            USB_RX_stuffing = false;
            USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
            break;
        }
    }

    //Temporizador de recepcion
    if (nbytes > 0) {
        USB_RX_byte_timer = clock();
    }else if (USB_RX_estado != HSCAR_COMM_ESTADO_CABECERA){
        uint32_t timeElapsed = ((double)(clock() - USB_RX_byte_timer))/CLOCKS_PER_SEC * 1000;

        if (timeElapsed > USB_RX_MAX_BYTE_DELAY) {
            USB_RX_estado = HSCAR_COMM_ESTADO_CABECERA;
            USB_RX_indice = 0;
            USB_RX_recibiendo = false;

            printf("\nTimeout byte");
            fflush(stdout);
        }
    }
}

/*!
  \brief Envia comando de STOP al coche
  \returns True si la comunicacion es correcta o False en caso contrario
  */
boolean enviarStop() {
    USB_TX_pila[0] = HSCAR_COMM_COMANDO_STOP;

    enviarUSB();
    return esperaACK();
}

/*!
  \brief Espera la llegada de un comando ACK
  \returns True si se recibe correctamente o False en caso contrario
  */
boolean esperaACK() {
    USB_RX_recibiendo = true;

    do {
        USB_RX_bytes_disponibles = read(USB_port, USB_RX_buffer, 255);
        leerUSB(USB_RX_bytes_disponibles);
    } while (USB_RX_recibiendo);

    return ((USB_RX_pila[0] == HSCAR_COMM_COMANDO_ACK) && USB_RX_crc_OK);
}

/*!
  \brief Envia el comando de lectura de las barras y espera una respuesta con los datos
  \param datos Puntero a array donde se introduciran los datos obtenidos
  \returns Cantidad de bytes recibida, 0 si error.
  */
uint32_t enviarLecturaBarras(char *datos) {
    USB_TX_pila[0] = HSCAR_COMM_COMANDO_LEE_BARRAS;

    enviarUSB();

    return leerComandoLargo(datos);
}

void vaciaBufferLectura() {
    uint32_t mierda[255];
    int leidos;

    while (leidos = read(USB_port, mierda, 255) == 255)
        qDebug() << "Vaciando buffer puerto serie";
}

/*!
  \brief Lee del USB un comando largo
  \param datos Puntero a array donde se introduciran los datos obtenidos
  \returns Cantidad de bytes recibida, 0 si error
  */
uint32_t leerComandoLargo(char *datos) {
    USB_RX_recibiendo = true;
    USB_RX_byte_timer = clock();

    while (USB_RX_recibiendo) {
        USB_RX_bytes_disponibles = read(USB_port, USB_RX_buffer, 255);

        if (USB_RX_bytes_disponibles < 1) {
            uint32_t timeElapsed = ((double)(clock() - USB_RX_byte_timer))/CLOCKS_PER_SEC * 1000;

            USB_RX_recibiendo = (timeElapsed <= USB_RX_MAX_RESP_DELAY);
        }

        leerUSB(USB_RX_bytes_disponibles);
    }

    if ((HSCAR_COMM_ES_COMANDO_LARGO(USB_RX_pila[0]))/* && USB_RX_crc_OK*/) {

        for (uint32_t i = 0; i < USB_RX_pila[1]; i++) {
            datos[i] = (0xFF & USB_RX_pila[i + 3]);
        }

        return USB_RX_pila[1];

    }else {
        return 0;
    }
}

void escribirLoopBack(char *datos, uint32_t longitud) {
    USB_TX_pila[0] = HSCAR_COMM_COMANDO_LOOPBACK;
    USB_TX_pila[1] = longitud;
    USB_TX_pila[2] = (~longitud) & 0xFF;

    for (uint32_t i = 0; i < longitud; i++) {
        USB_TX_pila[3 + i] = (0xFF & (uint32_t)datos[i]);
    }

    enviarUSB();
}

uint32_t CRC_CalcCRC(uint32_t data)
{
    uint32_t CRC = 0xFFFFFFFF;
    CRC = Crc32Fast(CRC, (uint32_t)data);
    return (CRC & 0xFF);
}

uint32_t CRC_CalcBlockCRC(uint32_t *data, uint32_t length)
{
    uint32_t CRC = 0xFFFFFFFF;

    for (uint32_t i=0 ; i< length; i++)
    {
        CRC = Crc32Fast(CRC, ((uint32_t)data[i]) & 0xFF);
    }
    return (CRC & 0xFF);
}

uint32_t Crc32Fast(uint32_t Crc, uint32_t Data)
{
    static const uint32_t CrcTable[16] = { // Nibble lookup table for 0x04C11DB7 polynomial
                                           0x00000000,0x04C11DB7,0x09823B6E,0x0D4326D9,0x130476DC,0x17C56B6B,0x1A864DB2,0x1E475005,
                                           0x2608EDB8,0x22C9F00F,0x2F8AD6D6,0x2B4BCB61,0x350C9B64,0x31CD86D3,0x3C8EA00A,0x384FBDBD };

    Crc = Crc ^ Data; // Apply all 32-bits

    // Process 32-bits, 4 at a time, or 8 rounds

    Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // Assumes 32-bit reg, masking index to 4-bits
    Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32
    Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
    Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
    Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
    Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
    Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
    Crc = (Crc << 4) ^ CrcTable[Crc >> 28];

    return(Crc);
}


void Solicitar_Variable(uint varid)
{
    USB_TX_pila[0] = HSCAR_COMM_COMANDO_SOLICITA_VAR;
    USB_TX_pila[1] = 1;
    USB_TX_pila[2] = 0xFF & ~USB_TX_pila[1];
    USB_TX_pila[3] = varid;
    enviarUSB();
}

//uint32_t Recibir_Variable(void)
//{
//    uint32_t varid;
//    varid = USART1_pila_RX[3];
//    switch(varid)
//    {
//        case varid_Kp_gas:
//            Kp_gas = *((float*)(&USART1_pila_RX[4]));
//            break;
//        case varid_Ki_gas:
//            Ki_gas = *((float*)(&USART1_pila_RX[4]));
//            break;
//        case varid_Kd_gas:
//            Kd_gas = *((float*)(&USART1_pila_RX[4]));
//            break;
//        default:
//            break;
//    }
//    return varid;
//}

//void Enviar_Variable(uint varid)
//{
//    int i;
//    char *punt_char;
//    USART1_pila_TX[0]=USART1_COMANDO_ENVIA_VAR;
//    switch(varid)
//    {
//        case varid_Kp_gas:
//            USART1_pila_TX[1]=sizeof(Kp_gas)+1;
//            punt_char = (char*)&Kp_gas;
//            break;
//        case varid_Ki_gas:
//            USART1_pila_TX[1]=sizeof(Ki_gas)+1;
//            punt_char = (char*)&Ki_gas;
//            break;
//        case varid_Kd_gas:
//            USART1_pila_TX[1]=sizeof(Kd_gas)+1;
//            punt_char = (char*)&Kd_gas;
//            break;
//        default:
//            break;
//    }
//    USART1_pila_TX[2]=0xFF & ~USART1_pila_TX[1];
//    USART1_pila_TX[3]=0xFF & varid;
//    for (i=4;i<sizeof(Kd_gas)+4;i++)
//    {
//        USART1_pila_TX[i]=*(punt_char++);
//    }
//    USART_ITConfig(USART1, USART_IT_TXE , ENABLE);
//}
